using BepuPhysics.Collidables;
using BepuUtilities;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;

namespace BepuPhysics
{
    // 您可以将这两个文件按位打包为4个字节,但其值非常值得怀疑。
    public struct BodyConstraintReference
    {
        public ConstraintHandle ConnectingConstraintHandle;
        public int BodyIndexInConstraint;
    }

    /// <summary>
    /// 存储一组实体-激活实体集或非活动模拟孤岛中涉及的实体。
    /// </summary>
    public struct BodySet
    {
        // 请注意,所有正文信息都以AOS格式存储。
        // 虽然姿势积分器在技术上将受益于(AO)SOA,但它只会在神奇的无限带宽场景中有所帮助。
        // 在实践中,姿势积分器的实际AOSOA受益块甚至不能扩展到2个线程,即使只有4个宽的SIMD。
        // 最重要的是,窄相位和解算器都需要以非连续的方式访问身体信息。虽然布局优化器阶段在这里可以在一定程度上有所帮助,
        // 简单的事实是,分散的加载可能会浪费大量的缓存线空间-即使对于更宽的SIMD包来说,大部分空间也是如此。
        // (考虑一下：AVX512 AOSOA布局上的非连续采样速度.Linear.X将加载一个64字节的缓存线,并且只使用其中的4个字节！)

        // 另外,在编写游戏逻辑时,没有人想要处理AOSOA布局。实际上,主体数据将是引擎中访问最频繁的属性,
        // 而且不需要做转位就可以把它拉进AOS,这就不那么痛苦了。

        /// <summary>
        /// 将正文索引重新映射到其句柄。
        /// </summary>
        public Buffer<BodyHandle> IndexToHandle;

        public Buffer<RigidPose> Poses;
        public Buffer<BodyVelocity> Velocities;
        public Buffer<BodyInertia> LocalInertias;

        /// <summary>
        /// 集合中每个物体拥有的可碰撞物体。可以直接更改推测边距、连续性设置和形状指数。
        /// 在没有通知可碰撞添加或移除的大阶段的情况下,形状索引不能在指向形状和指向任何形状之间转换,反之亦然。
        /// </summary>
        public Buffer<Collidable> Collidables;
        /// <summary>
        /// 集合中物体的活动状态。
        /// </summary>
        public Buffer<BodyActivity> Activity;
        /// <summary>
        /// 与集合中的每个主体关联的约束列表。
        /// </summary>
        public Buffer<QuickList<BodyConstraintReference>> Constraints;

        public int Count;
        /// <summary>
        /// 获取此实例是否由分配的内存支持。
        /// </summary>
        public bool Allocated { get { return IndexToHandle.Allocated; } }

        public BodySet(int initialCapacity, BufferPool pool) : this()
        {
            InternalResize(initialCapacity, pool);
        }

        internal int Add(in BodyDescription bodyDescription, BodyHandle handle, int minimumConstraintCapacity, BufferPool pool)
        {
            var index = Count;
            if (index == IndexToHandle.Length)
            {
                InternalResize(IndexToHandle.Length * 2, pool);
            }
            ++Count;
            IndexToHandle[index] = handle;
            // Collible的宽广相位指数没有设定。Body集合负责附加该数据。
            Constraints[index] = new QuickList<BodyConstraintReference>(minimumConstraintCapacity, pool);
            ApplyDescriptionByIndex(index, bodyDescription);
            return index;
        }

        internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIndex, out BodyHandle movedBodyHandle)
        {
            handle = IndexToHandle[bodyIndex];
            // 将最后一个躯干移动到移除的插槽中。
            --Count;
            bool bodyMoved = bodyIndex < Count;
            if (bodyMoved)
            {
                movedBodyIndex = Count;
                // 将最后一个元素的内存状态向下复制。
                Poses[bodyIndex] = Poses[movedBodyIndex];
                Velocities[bodyIndex] = Velocities[movedBodyIndex];
                LocalInertias[bodyIndex] = LocalInertias[movedBodyIndex];
                Activity[bodyIndex] = Activity[movedBodyIndex];
                Collidables[bodyIndex] = Collidables[movedBodyIndex];
                // 请注意,约束列表在被覆盖之前不会被释放。
                // 此函数的两个调用方是"true"删除和休眠。
                // 在真正的删除过程中,调用方负责删除所有约束并处置列表。
                // 在休眠时,对列表的引用只是复制到休眠集中。
                Constraints[bodyIndex] = Constraints[movedBodyIndex];
                // 将主体操纵柄指向新位置。
                movedBodyHandle = IndexToHandle[movedBodyIndex];
                IndexToHandle[bodyIndex] = movedBodyHandle;
            }
            else
            {
                movedBodyIndex = -1;
                movedBodyHandle = new BodyHandle(-1);
            }
            return bodyMoved;
        }

        internal void ApplyDescriptionByIndex(int index, in BodyDescription description)
        {
            Debug.Assert(!MathChecker.IsInvalid(description.Pose.Position.LengthSquared()), $"Invalid body position: {description.Pose.Position}");
            Debug.Assert(Math.Abs(1 - description.Pose.Orientation.LengthSquared()) < 1e-3f, $"Body orientation not unit length: {description.Pose.Orientation}");
            Debug.Assert(!MathChecker.IsInvalid(description.Velocity.Linear.LengthSquared()), $"Invalid body linear velocity: {description.Velocity.Linear}");
            Debug.Assert(!MathChecker.IsInvalid(description.Velocity.Angular.LengthSquared()), $"Invalid body angular velocity: {description.Velocity.Angular}");
            Debug.Assert(!MathChecker.IsInvalid(
                description.LocalInertia.InverseInertiaTensor.XX * description.LocalInertia.InverseInertiaTensor.XX +
                description.LocalInertia.InverseInertiaTensor.YX * description.LocalInertia.InverseInertiaTensor.YX +
                description.LocalInertia.InverseInertiaTensor.YY * description.LocalInertia.InverseInertiaTensor.YY +
                description.LocalInertia.InverseInertiaTensor.ZX * description.LocalInertia.InverseInertiaTensor.ZX +
                description.LocalInertia.InverseInertiaTensor.ZY * description.LocalInertia.InverseInertiaTensor.ZY +
                description.LocalInertia.InverseInertiaTensor.ZZ * description.LocalInertia.InverseInertiaTensor.ZZ), $"Invalid body inverse inertia tensor: {description.LocalInertia.InverseInertiaTensor}");
            Debug.Assert(!MathChecker.IsInvalid(description.LocalInertia.InverseMass) && description.LocalInertia.InverseMass >= 0, $"Invalid body inverse mass: {description.LocalInertia.InverseMass}");

            Poses[index] = description.Pose;
            Velocities[index] = description.Velocity;
            LocalInertias[index] = description.LocalInertia;
            ref var collidable = ref Collidables[index];
            collidable.Continuity = description.Collidable.Continuity;
            collidable.SpeculativeMargin = description.Collidable.SpeculativeMargin;
            // 请注意,我们在这里更改了形状。如果可碰撞的过渡从无形状->有形状或有形状->无形状->发生,则必须通知宽相位
            // 以便它可以创建/删除条目。这就是该函数不公开的原因。
            collidable.Shape = description.Collidable.Shape;
            ref var activity = ref Activity[index];
            activity.SleepThreshold = description.Activity.SleepThreshold;
            activity.MinimumTimestepsUnderThreshold = description.Activity.MinimumTimestepCountUnderThreshold;
            activity.TimestepsUnderThresholdCount = 0;
            activity.SleepCandidate = false;
        }

        public void GetDescription(int index, out BodyDescription description)
        {
            description.Pose = Poses[index];
            description.Velocity = Velocities[index];
            description.LocalInertia = LocalInertias[index];
            ref var collidable = ref Collidables[index];
            description.Collidable.Continuity = collidable.Continuity;
            description.Collidable.Shape = collidable.Shape;
            description.Collidable.SpeculativeMargin = collidable.SpeculativeMargin;
            ref var activity = ref Activity[index];
            description.Activity.SleepThreshold = activity.SleepThreshold;
            description.Activity.MinimumTimestepCountUnderThreshold = activity.MinimumTimestepsUnderThreshold;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool)
        {
            BodyConstraintReference constraint;
            constraint.ConnectingConstraintHandle = constraintHandle;
            constraint.BodyIndexInConstraint = bodyIndexInConstraint;
            ref var constraints = ref Constraints[bodyIndex];
            Debug.Assert(constraints.Span.Allocated, "Any time a body is created, a list should be built to support it.");
            if (constraints.Span.Length == constraints.Count)
                constraints.Resize(constraints.Span.Length * 2, pool);
            constraints.AllocateUnsafely() = constraint;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool)
        {
            // 这使用线性搜索。这很好;身体很少会有超过几个与它们相关的约束。
            // 尝试使用类似散列集的方式进行快速删除只会带来更多的持续开销,并且平均而言会减慢它的运行速度。
            ref var list = ref Constraints[bodyIndex];
            for (int i = 0; i < list.Count; ++i)
            {
                ref var element = ref list[i];
                if (element.ConnectingConstraintHandle.Value == constraintHandle.Value)
                {
                    list.FastRemoveAt(i);
                    break;
                }
            }
            // 请注意保守的调整大小阈值。如果当前容量是8,最小容量是4,并且当前计数是4,则它可以调整大小,
            // 但这不会做到这一点。相反,它将等待另一次减半-当前计数需要为2,才允许容量降至4。
            // 这有助于避免在约束快速搅拌时过度调整大小。
            var conservativeCount = 2 * list.Count;
            var targetCapacity = conservativeCount > minimumConstraintCapacityPerBody ? conservativeCount : minimumConstraintCapacityPerBody;
            // 如果它最终只会是2的幂,那就别费心去调整大小了。
            if (list.Span.Length >= 2 * targetCapacity)
            {
                // 可以在保留所有现有约束并遵守最小容量的情况下对列表进行小幅削减。
                list.Resize(targetCapacity, pool);
            }
        }

        public bool BodyIsConstrainedBy(int bodyIndex, ConstraintHandle constraintHandle)
        {
            ref var list = ref Constraints[bodyIndex];
            for (int i = 0; i < list.Count; ++i)
            {
                if (list[i].ConnectingConstraintHandle.Value == constraintHandle.Value)
                {
                    return true;
                }
            }
            return false;
        }


        /// <summary>
        /// 交换两具身体的记忆。按内存插槽索引,而不是按句柄索引。
        /// </summary>
        /// <param name="Slota">要交换的第一个正文的内存插槽。</param>
        /// <param name="slotB">要交换的第二个主体的内存插槽。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal void Swap(int slotA, int slotB, ref Buffer<BodyMemoryLocation> handleToIndex)
        {
            handleToIndex[IndexToHandle[slotA].Value].Index = slotB;
            handleToIndex[IndexToHandle[slotB].Value].Index = slotA;
            Helpers.Swap(ref IndexToHandle[slotA], ref IndexToHandle[slotB]);
            Helpers.Swap(ref Collidables[slotA], ref Collidables[slotB]);
            Helpers.Swap(ref Poses[slotA], ref Poses[slotB]);
            Helpers.Swap(ref Velocities[slotA], ref Velocities[slotB]);
            Helpers.Swap(ref LocalInertias[slotA], ref LocalInertias[slotB]);
            Helpers.Swap(ref Activity[slotA], ref Activity[slotB]);
            Helpers.Swap(ref Constraints[slotA], ref Constraints[slotB]);
        }

        internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool)
        {
            Debug.Assert(targetBodyCapacity > 0, "Resize is not meant to be used as Dispose. If you want to return everything to the pool, use Dispose instead.");
            // 请注意,我们将包容量基于调整IndexToHandle数组的大小后的容量。这简化了分配条件,但增加了内存使用。
            // 如果考虑到内存使用,您可能希望在将来更改此设置。
            targetBodyCapacity = BufferPool.GetCapacityForCount<int>(targetBodyCapacity);
            Debug.Assert(Poses.Length != BufferPool.GetCapacityForCount<RigidPoses>(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size.");
            pool.ResizeToAtLeast(ref Poses, targetBodyCapacity, Count);
            pool.ResizeToAtLeast(ref Velocities, targetBodyCapacity, Count);
            pool.ResizeToAtLeast(ref LocalInertias, targetBodyCapacity, Count);
            pool.ResizeToAtLeast(ref IndexToHandle, targetBodyCapacity, Count);
            pool.ResizeToAtLeast(ref Collidables, targetBodyCapacity, Count);
            pool.ResizeToAtLeast(ref Activity, targetBodyCapacity, Count);
            pool.ResizeToAtLeast(ref Constraints, targetBodyCapacity, Count);
        }

        public unsafe void Clear(BufferPool pool)
        {
            for (int i = 0; i < Count; ++i)
            {
                Constraints[i].Dispose(pool);
            }
            Count = 0;
        }

        /// <summary>
        /// 释放缓冲区,但不释放缓冲区内的任何内容。不会返回存储在集合中的每主体约束列表。
        /// </summary>
        /// <param name="pool">要将集合的顶级缓冲区返回到的池。</param>
        public void DisposeBuffers(BufferPool pool)
        {
            pool.Return(ref Poses);
            pool.Return(ref Velocities);
            pool.Return(ref LocalInertias);
            pool.Return(ref IndexToHandle);
            pool.Return(ref Collidables);
            pool.Return(ref Activity);
            pool.Return(ref Constraints);
        }

        /// <summary>
        /// 释放主体集的缓冲区和其中的任何资源。
        /// </summary>
        /// <param name="pool">要将资源返回到的池。</param>
        public void Dispose(BufferPool pool)
        {
            for (int i = 0; i < Count; ++i)
            {
                Constraints[i].Dispose(pool);
            }
            DisposeBuffers(pool);
            this = new BodySet();
        }
    }
}
